/**
* @file        SensorDataCache
* @brief       
* @version     1.0.0
* @author      Changlin.Jing 
* @date        2022/10/19 19:57
* @copyright   Copyright  2022 Langge Software Co., Ltd. All rights reserved.
*/
#include "SensorDataCache.hpp"
#include "ut_algorithm.h"
#include "ut_log_macros.h"
#include <cmath>
#include <numeric>
vdr::SensorDataCache * vdr::SensorDataCache::mInstance = nullptr;
vdr::SensorDataCache *vdr::SensorDataCache::GetInstance() {
    if(nullptr == mInstance) {
        mInstance = new SensorDataCache;
    }
    return mInstance;
}
vdr::SensorDataCache::SensorDataCache() {
    reset();
}

vdr::SensorDataCache::~SensorDataCache() {
    if(nullptr!=mRawCache){
        delete mRawCache;
        mRawCache = nullptr;
    }
}

void vdr::SensorDataCache::reset() {
    mLocalTickTime = 0;
    mPosInit = false;
    if(nullptr != mRawCache){
        delete mRawCache;
        mRawCache = nullptr;
    }

    mRawCache = new VectorBuffer<std::uint64_t ,double>(mCacheDim,sCacheSize, true);
    mRawCache->reset();
    mLastRaw.resize(mCacheDim,0.0);
    mLlo.resize(2,0.0);
    mFlatXy.resize(2,0.0);

    mSensorInit = false;
    mAccLastTimeStamp = -1;
    mGyrLastTimeStamp = -1;
    mMagLastTimeStamp = -1;
    mRawLastTimeStamp = -1;

    mIsAccFull = false;
    mIsGyrFull = false;
    mIsMagFull = false;

    mIndexAcc = 0;
    mIndexGyr = 0;
    mIndexMag = 0;

    mLastAcc = {0.0,0.0,-9.81,0.0};
    mLastGyr = {0.0,0.0,0.0};
    mLastMag = {0.0,0.0,0.0};

    mLastTurnRate = 0.0;
    tureRateK = 0.8;

    mAccHorizonData.resize(mSizeWindow,0);
    mAccVerticalData.resize(mSizeWindow,0);
    mAccTotalData.resize(mSizeWindow,0);
    mAccYOriginData.resize(mSizeWindow, 0);

    mAccHorizonDataDelta.resize(mSizeWindow,0);
    mAccVerticalDataDelta.resize(mSizeWindow,0);
    mAccTotalDataDelta.resize(mSizeWindow,0);

    mAccBearingData.resize(mSizeWindow,0);

    mGyrVerticalData.resize(mSizeWindow,0);
    mGyrHorizonData.resize(mSizeWindow,0);
    mGyrTotalData.resize(mSizeWindow,0);
    mGyrVerticalDataDelta.resize(mSizeWindow,0);
    mGyrHorizonDataDelta.resize(mSizeWindow,0);
    mGyrTotalDataDelta.resize(mSizeWindow,0);

    mMagHorizonData.resize(mSizeWindow,0);
    mMagVerticalData.resize(mSizeWindow,0);
    mMagTotalData.resize(mSizeWindow,0);
    mMagHorizonDataDelta.resize(mSizeWindow,0);
    mMagVerticalDataDelta.resize(mSizeWindow,0);
    mMagTotalDataDelta.resize(mSizeWindow,0);
    //车速标准化10Hz，存3s左右
    mCarData.resize(mCarSizeWindow, 0);

    mAccGpsBearingDeltaData.resize(mSizeWindow,0);
}

void vdr::SensorDataCache::storeData(const std::shared_ptr<I_LOC_SIGNAL>& posSignal) {
    mLocalTickTime = posSignal->mLocalTickTime;
    // 缓存原始GNSS 信号
    if(POS_SIGNAL == posSignal->mLocalSignalType){
        std::shared_ptr<LOC_POS_SIGNAL> signalPos = std::static_pointer_cast<LOC_POS_SIGNAL>(posSignal);

        // 先初始化，初始化把 llo 存下来
        if(!mPosInit ){
            initPos(signalPos);
            mPosInit = true;
        }
        addRawSignal(signalPos);

    }

    if(ROTATION_SIGNAL == posSignal->mLocalSignalType){
        std::shared_ptr<LOC_ROTATION_SIGNAL> rotationSignal = std::static_pointer_cast<LOC_ROTATION_SIGNAL>(posSignal);
        mRotation = rotationSignal;
        for(int i=0;i<9;i++){
            mRotationMatrix.at(i) = mRotation->mRotation[i];
        }
    }

    if(ACC_SIGNAL == posSignal->mLocalSignalType){
        std::shared_ptr<LOC_ACC_SIGNAL> accSignal = std::static_pointer_cast<LOC_ACC_SIGNAL>(posSignal);
        if(mRotation!= nullptr){
            accSignal->mStandX = mRotationMatrix[0] * accSignal->mOriginX + mRotationMatrix[1] * accSignal->mOriginY + + mRotationMatrix[2] * accSignal->mOriginZ;
            accSignal->mStandY = mRotationMatrix[3] * accSignal->mOriginX + mRotationMatrix[4] * accSignal->mOriginY + + mRotationMatrix[5] * accSignal->mOriginZ;
            accSignal->mStandZ = mRotationMatrix[6] * accSignal->mOriginX + mRotationMatrix[7] * accSignal->mOriginY + + mRotationMatrix[8] * accSignal->mOriginZ;
        }
        mLastAccSignal = accSignal;
        addAccSignal(accSignal);
    }

    if(GYRO_SIGNAL == posSignal->mLocalSignalType){
        std::shared_ptr<LOC_GYRO_SIGNAL> gyrSignal = std::static_pointer_cast<LOC_GYRO_SIGNAL>(posSignal);
        if(mRotation!= nullptr){
            gyrSignal->mStandX = mRotationMatrix[0] * gyrSignal->mOriginX + mRotationMatrix[1] * gyrSignal->mOriginY + + mRotationMatrix[2] * gyrSignal->mOriginZ;
            gyrSignal->mStandY = mRotationMatrix[3] * gyrSignal->mOriginX + mRotationMatrix[4] * gyrSignal->mOriginY + + mRotationMatrix[5] * gyrSignal->mOriginZ;
            gyrSignal->mStandZ = mRotationMatrix[6] * gyrSignal->mOriginX + mRotationMatrix[7] * gyrSignal->mOriginY + + mRotationMatrix[8] * gyrSignal->mOriginZ;
        }
        mLastGyroSignal = gyrSignal;
        addGyrSignal(gyrSignal);

    }
    if(MAGN_SIGNAL == posSignal->mLocalSignalType){
        std::shared_ptr<LOC_MAGN_SIGNAL> magSignal = std::static_pointer_cast<LOC_MAGN_SIGNAL>(posSignal);
        addMagSignal(magSignal);
    }
    if(SATELLITE_SIGNAL == posSignal->mLocalSignalType){
        std::shared_ptr<LOC_SATELLITE_SIGNAL> satelliteSignal = std::static_pointer_cast<LOC_SATELLITE_SIGNAL>(posSignal);
        mPtrLastSatelliteSignal =satelliteSignal;
    }
    if (CAR_SIGNAL == posSignal->mLocalSignalType) {
        std::shared_ptr<LOC_CAR_SIGNAL> carSignal = std::static_pointer_cast<LOC_CAR_SIGNAL>(posSignal);
        //执行数据缓存
        addCarSignal(carSignal);
    }
}

void vdr::SensorDataCache::initPos(const std::shared_ptr<LOC_POS_SIGNAL>& signal) {
    mLocalTickTime = signal->mLocalTickTime;
    mLlo[0] = signal->mLongitude;
    mLlo[1] = signal->mLatitude;
    mPosInit = true;
}

void vdr::SensorDataCache::addRawSignal(const std::shared_ptr<LOC_POS_SIGNAL>& signal) {

    std::vector<double> res;
    UTAlgorithm::Lla2Enu(res,signal->mLatitude,signal->mLongitude,0,mLlo[1],mLlo[0],0);
    mFlatXy[0] = res[0];
    mFlatXy[1] = res[1];

    double tmpRadian = UTAlgorithm::ToRadians(signal->mHeading);

    if(mLastRaw.empty() || mLastPosSignal == nullptr){
        std::vector<double> tmp{mFlatXy[0],mFlatXy[1],signal->mSpeed,cos(tmpRadian),sin(tmpRadian),signal->mHeading, signal->mHeading,cos(tmpRadian),sin(tmpRadian),0,0};
        this->mRawCache->addData(signal->mLocalTickTime,tmp);
        mLastRaw = tmp;
    }else{
        double distance = UTAlgorithm::CalculateDistance(mLastRaw[0],mLastRaw[1],mFlatXy[0],mFlatXy[1]);
        double speed = distance /((signal->mLocalTickTime - mRawLastTimeStamp)/1000.0);
        double cosPos = (mFlatXy[0] - mLastRaw[0])/distance;
        double sinPos = (mFlatXy[1] - mLastRaw[1])/distance;
        double posBearing = UTAlgorithm::NormalizeDegrees(UTAlgorithm::ToDegrees(atan(mFlatXy[1] - mLastRaw[1]/(mFlatXy[0]-mLastRaw[0]))));
        std::vector<double> tmp{mFlatXy[0],mFlatXy[1],signal->mSpeed,cos(tmpRadian),sin(tmpRadian),signal->mHeading,posBearing,cosPos,sinPos,distance,speed};
        this->mRawCache->addData(signal->mLocalTickTime,tmp);
        mLastRaw = tmp;
    }
    mLastPosSignal = signal;
    mRawLastTimeStamp = signal->mLocalTickTime;
}

void vdr::SensorDataCache::addAccSignal(const std::shared_ptr<LOC_ACC_SIGNAL>& signal) {
    if(nullptr == mRotation) return;

    /// 加速度计数据是否中断超过一秒，超过后重制
    if(mAccLastTimeStamp !=-1 && signal->mLocalTickTime - mAccLastTimeStamp > mSensorTimeout){
        mIsAccFull = false;
        mIndexAcc = 0;
    }

    mAccLastTimeStamp = signal->mLocalTickTime;

    mLastOriginAcc = {signal->mOriginX,signal->mOriginY,signal->mOriginZ};
    float totalAcc = UTAlgorithm::CalcNorm(mLastOriginAcc);
    mAccYOriginData[mIndexAcc] = mLastOriginAcc[1];
    if(totalAcc > 2 * gravity){
        mAccHorizonData[mIndexAcc] = mLastAcc[0];
        mAccVerticalData[mIndexAcc] = mLastAcc[1];
        mAccTotalData[mIndexAcc] = mLastAcc[2];
        mAccBearingData[mIndexAcc] = mLastAcc[3];
    }else{
        std::vector<float> rotatedAcc = UTAlgorithm::Rotate(mRotationMatrix,mLastOriginAcc);
        mAccVerticalData[mIndexAcc] = rotatedAcc[2];
        mAccHorizonData[mIndexAcc] = std::sqrt(rotatedAcc[0]*rotatedAcc[0] + rotatedAcc[1]*rotatedAcc[1]);
        mAccTotalData[mIndexAcc] = totalAcc;
        mAccBearingData[mIndexAcc] =  UTAlgorithm::ToDegrees(atan2(rotatedAcc[1],rotatedAcc[0]));
    }

    if(mLastRaw.empty()){
        mAccGpsBearingDeltaData[mIndexAcc] = 0.0;
    }else{
        mAccGpsBearingDeltaData[mIndexAcc] = UTAlgorithm::CalcDegreeDiff(mAccBearingData[mIndexAcc],mLastRaw[3]);
    }

    mLastAcc[0] = mAccHorizonData[mIndexAcc];
    mLastAcc[1] = mAccVerticalData[mIndexAcc];
    mLastAcc[2] = mAccTotalData[mIndexAcc];
    mLastAcc[3] = mAccBearingData[mIndexAcc];

    if(!mIsAccFull && mIndexAcc == mSizeWindow-1){
        mIsAccFull = true;
    }
    mIndexAcc = (mIndexAcc+1)%mSizeWindow;
}

void vdr::SensorDataCache::addGyrSignal(const std::shared_ptr<LOC_GYRO_SIGNAL>& signal) {
    if(nullptr == mRotation) return;

    if(mMagLastTimeStamp != -1 && signal->mLocalTickTime - mMagLastTimeStamp > mSensorTimeout){
        mIsMagFull = false;
        mIndexMag = 0;
    }

    mMagLastTimeStamp = signal->mLocalTickTime;

    mLastOriginMag = {signal->mOriginX,signal->mOriginY,signal->mOriginZ};
    std::vector<float> rotatedMag = UTAlgorithm::Rotate(mRotationMatrix,mLastOriginMag);

    mMagVerticalData[mIndexMag] = rotatedMag[2];
    mMagHorizonData[mIndexMag] = std::sqrt(rotatedMag[0]+rotatedMag[0]+rotatedMag[1]*rotatedMag[1]);
    mMagTotalData[mIndexMag] = UTAlgorithm::CalcNorm(mLastOriginMag);

    mMagHorizonDataDelta[mIndexMag] = mMagHorizonData[mIndexMag] - mLastMag[0];
    mMagVerticalDataDelta[mIndexMag] = mMagVerticalData[mIndexMag] - mLastMag[1];
    mMagTotalDataDelta[mIndexMag] = mMagTotalData[mIndexMag] - mLastMag[2];

    mLastMag[0] = mMagHorizonData[mIndexMag];
    mLastMag[1] = mMagVerticalData[mIndexMag];
    mLastMag[2] = mMagTotalData[mIndexMag];

    if(!mIsMagFull && mIndexMag == mSizeWindow-1){
        mIsMagFull = true;
    }
    mIndexMag = (mIndexMag +1 )% mSizeWindow;
}

void vdr::SensorDataCache::addMagSignal(const std::shared_ptr<LOC_MAGN_SIGNAL>& signal) {
    if(nullptr == mRotation) return;

    if(mGyrLastTimeStamp !=-1 && signal->mLocalTickTime - mGyrLastTimeStamp > mSensorTimeout){
        mIsAccFull = false;
        mIndexGyr = 0;
    }

    mGyrLastTimeStamp = signal->mLocalTickTime;

    mLastOriginGyr = {signal->mOriginX,signal->mOriginY,signal->mOriginZ};

    std::vector<float> rotationGyr = UTAlgorithm::Rotate(mRotationMatrix,mLastOriginGyr);

    mGyrVerticalData[mIndexGyr] = rotationGyr[2];

    mGyrHorizonData[mIndexGyr] = rotationGyr[0] * rotationGyr[0] + rotationGyr[1]*rotationGyr[1];
    mGyrTotalData[mIndexGyr] = UTAlgorithm::CalcNorm(mLastOriginGyr);

    mGyrHorizonDataDelta[mIndexGyr] = mGyrHorizonData[mIndexGyr] - mLastGyr[0];
    mGyrVerticalDataDelta[mIndexGyr] = mGyrVerticalData[mIndexGyr] - mLastGyr[1];
    mGyrTotalDataDelta[mIndexGyr] = mGyrTotalData[mIndexGyr] - mLastGyr[2];

    mLastTurnRate = tureRateK * mLastTurnRate + (1-tureRateK)*mLastGyr[1];

    if(!mIsGyrFull && mIndexGyr == mSizeWindow -1){
        mIsGyrFull = true;
    }

    mIndexGyr = (mIndexGyr + 1) % mSizeWindow;
}

std::vector<float> vdr::SensorDataCache::getAHRS() const {
    if(this->mRotation!= nullptr){
        std::vector<float> tmp{this->mRotation->mX,this->mRotation->mY,this->mRotation->mZ};
        return tmp;
    }
    return {};
}

void vdr::SensorDataCache::ReleaseInstance() {
    if(nullptr!=mInstance){
        delete mInstance;
        mInstance = nullptr;
    }
}

void vdr::SensorDataCache::CheckWorkStateBySensor() {
    if (hasSensorWorkStateCheck) return;
    //检查到车速，自动转成车机模式
    if (mIsCarFull && !mCarData.empty()) {
        isPlatformAuto = true;
        hasSensorWorkStateCheck = true;
    }
    //检车到加速度Y轴方向反向
    if (mIsAccFull && !mAccVerticalData.empty()) {
        float meanAccY = std::accumulate(std::begin(mAccYOriginData), std::end(mAccYOriginData), 0.0)/ int(mAccVerticalData.size());
        if (meanAccY < -8.5) {
            isPlatformAuto = true;
        }
        hasSensorWorkStateCheck = true;
    }
}

void vdr::SensorDataCache::addCarSignal(const std::shared_ptr<LOC_CAR_SIGNAL> &sigCar) {
    if (!sigCar->isSpeedValid()) return;
    CheckWorkStateBySensor();
    //车速信息丢失超 2s 重新缓存
    if (mCarLastTimeStamp != -1 && sigCar->mLocalTickTime - mCarLastTimeStamp > mSensorTimeout * 2) {
        mIsCarFull = false;
        mIndexCar = 0;
        mLastCarSPeed = -1.f;
    }
    mCarLastTimeStamp = sigCar->mLocalTickTime;
    mLastCarSignal = sigCar;
    mLastCarSPeed = mLastCarSignal->mRealSpeed;
    mCarData[mIndexCar] = sigCar->mRealSpeed;

    if (!mIsCarFull && mIndexCar == mCarSizeWindow - 1) {
        mIsCarFull = true;
    }
    mIndexCar = (mIndexCar + 1) % mCarSizeWindow;
}



